(Not applicable)
This invention relates generally to inertial navigation systems and more specifically to compensating acceleration measurements for body frame rotation and establishing the optimal center of navigation for a general accelerometer configuration.
Rotation creates centripetal and tangential accelerations which are proportional to the lever arm from the center of rotation to the rotating body. Any rotation through an arbitrary origin should produce zero acceleration at the origin. In a practical inertial navigation system, the three accelerometers cannot share the same origin. As a result, rotation about a reference origin can produce erroneous sensed accelerations and compensation is required.
In the past, the lever arms associated with the tangential acceleration (angular acceleration dependent) terms were small and produced negligible errors compared to the performance requirements. Compensation was based on centripetal (angular rate) terms only.
Some inertial navigation systems use silicon accelerometers mounted on the block in positions where angular acceleration terms produce errors that can approach the performance requirements. In the present invention a complete size effect compensation technique is used that treats both angular rate and angular acceleration terms. The algorithm performance is determined analytically, and used to define the optimal center of navigation for a general accelerometer configuration.
The notation used herein is as follows: a vector is denoted with an overhead arrow, e.g. {right arrow over (w)}, while a matrix is denoted in boldface, e.g. C.
The direction cosine matrix can be represented analytically by
CBN=exp(xcex5 sin(wt)[{right arrow over (1)}xcfx86x])xe2x80x83xe2x80x83(1)
and the rate of change of the direction cosine matrix by
{dot over (C)}BN=CBN[{right arrow over (w)}x]xe2x80x83xe2x80x83(2)
which means the sinusoidal angular velocity is given by
{right arrow over (w)}=W{right arrow over (1)}xcfx86xe2x80x83xe2x80x83(3)
W=xcex5wcos(wt)xe2x80x83xe2x80x83(4)
and angular acceleration is given by
                                          w            .                    →                =                                                            ⅆ                                  xe2x80x83                                                            ⅆ                t                                      ⁢                          xe2x80x83                        ⁢                          w              →                                =                                    -              ε                        ⁢                          xe2x80x83                        ⁢                          w              2                        ⁢                          xe2x80x83                        ⁢            sin            ⁢                          xe2x80x83                        ⁢                          (              wt              )                        ⁢                          xe2x80x83                        ⁢                                          1                →                            φ                                                          (        5        )            
where xcex5 is the magnitude of the angular displacement and w is the frequency about an arbitrary but fixed axis
{right arrow over (1)}xcfx86=[cxcycz]xe2x80x83xe2x80x83(6)
where
{square root over (cx2+cy2+cz2)}=1,xe2x80x83xe2x80x83(7)
Accelerometers with lever arm vector {right arrow over (R)} will sense an acceleration
{right arrow over (a)}B=([{right arrow over (w)}x]2+[{dot over ({right arrow over (w)})}x])RBxe2x80x83xe2x80x83(8)
where the cross product
{right arrow over (xcfx86)}x{right arrow over (R)}=[{right arrow over (xcfx86)}x]{right arrow over (R)}xe2x80x83xe2x80x83(9)
where                               [                                    φ              →                        ⁢                          xe2x80x83                        ⁢            x                    ]                =                  [                                                    0                                                              -                                      φ                    x                                                                                                φ                  y                                                                                                      φ                  x                                                            0                                                              -                                      φ                    x                                                                                                                        -                                      φ                    y                                                                                                φ                  x                                                            0                                              ]                                    (        10        )            
and the cross product
{right arrow over (xcfx86)}x[{right arrow over (xcfx86)}x{right arrow over (R)}]=[{right arrow over (xcfx86)}x]2{right arrow over (R)}xe2x80x83xe2x80x83(11)
where                                           [                                          φ                →                            ⁢                              xe2x80x83                            ⁢              x                        ]                    2                =                  [                                                                                          -                                          φ                      y                      2                                                        -                                      φ                    x                    2                                                                                                                    φ                    x                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    y                                                                                                                    φ                    x                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    z                                                                                                                                            φ                    x                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    y                                                                                                                    -                                          φ                      x                      2                                                        -                                      φ                    z                    2                                                                                                                    φ                    y                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    z                                                                                                                                            φ                    x                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    z                                                                                                                    φ                    y                                    ⁢                                      xe2x80x83                                    ⁢                                      φ                    z                                                                                                                    -                                          φ                      x                      2                                                        -                                                            φ                      y                      2                                        .                                                                                ]                                    (        12        )            
If all three accelerometers shared the same origin, the lever arm RB=0, and for a navigation center defined as that origin, any angular rate through that origin would not produce measured acceleration in any axis.
The projections on to the X, Y, and Z axis are the body frame accelerations, i.e.
{right arrow over (a)}x=Px([{right arrow over (w)}x]2+[{dot over ({right arrow over (w)})}x]){right arrow over (R)}xxe2x80x83xe2x80x83(13)
{right arrow over (a)}y=Py([{right arrow over (w)}x]2+[{dot over ({right arrow over (w)})}x]){right arrow over (R)}yxe2x80x83xe2x80x83(14)
{right arrow over (a)}z=Pz([{right arrow over (w)}x]2+[{dot over ({right arrow over (w)})}x]){right arrow over (R)}zxe2x80x83xe2x80x83(15)
where the projection matrices are                               P          x                =                  [                                                    1                                            0                                            0                                                                    0                                            0                                            0                                                                    0                                            0                                            0                                              ]                                    (        16        )                                          P          y                =                  [                                                    0                                            0                                            0                                                                    0                                            1                                            0                                                                    0                                            0                                            0                                              ]                                    (        17        )                                          P          z                =                  [                                                    0                                            0                                            0                                                                    0                                            0                                            0                                                                    0                                            0                                            1                                              ]                                    (        18        )            
and {right arrow over (R)}x, {right arrow over (R)}y, and {right arrow over (R)}z are the lever arm vectors for accelerometers in the X, Y, and Z directions respectively.
This is a convenient way to write the measured acceleration, since the other elements of the vector are zero, i.e.                                                         a              →                        x                    =                      [                                                                                a                                          B                      x                                                                                                                    0                                                                              0                                                      ]                          ,                                            a              →                        y                    =                      [                                                            0                                                                                                  a                                          B                      y                                                                                                                    0                                                      ]                          ,                                            a              →                        x                    =                      [                                                            0                                                                              0                                                                                                  a                                          B                      z                                                                                            ]                                              (        19        )            
which can also be written as dot products, i.e.
aBx={right arrow over (a)}xxc2x7{right arrow over (1)}x,aBy={right arrow over (a)}yxc2x7{right arrow over (1)}y,aBz={right arrow over (a)}zxc2x7{right arrow over (1)}zxe2x80x83xe2x80x83(20)
where the unit vectors are defined as                                                         1              →                        x                    =                      [                                                            1                                                                              0                                                                              0                                                      ]                          ,                                            1              →                        y                    =                                    [                                                                    0                                                                                        1                                                                                        0                                                              ]                        T                          ,                                            1              →                        x                    =                                    [                                                                    0                                                                                        0                                                                                        1                                                              ]                        T                                              (        21        )            
Using the same matrix representations for cross products shown in Equation (10) and Equation (16) but substituting
{right arrow over (w)}=[wxwywz]xe2x80x83xe2x80x83(22)
{dot over ({right arrow over (w)})}=[{dot over (w)}x{dot over (w)}y{dot over (w)}z]xe2x80x83xe2x80x83(23)
where
wx=Wcxxe2x80x83xe2x80x83(24)
wy=Wcyxe2x80x83xe2x80x83(25)
wz=Wczxe2x80x83xe2x80x83(26)
and for lever arms
{right arrow over (R)}x=[RxxRyxRzx]xe2x80x83xe2x80x83(27)
{right arrow over (R)}y=[RxyRyyRzy]xe2x80x83xe2x80x83(28)
{right arrow over (R)}z=[RxzRyzRzz]xe2x80x83xe2x80x83(29)
where Rij is the i""th coordinate of the j""th accelerometer, the acceleration is given by
aBx=(xe2x88x92wy2xe2x88x92wz2)Rxx+(xe2x88x92{dot over (w)}z+wxwy)Ryz+({dot over (w)}y+wxwz)Rzxxe2x80x83xe2x80x83(30)
xe2x80x83aBy=({dot over (w)}z+wxwy)Rxy+(xe2x88x92wx2xe2x88x92wx2)Ryy+(xe2x88x92{dot over (w)}x+wywz)Rzyxe2x80x83xe2x80x83(31)
aBz=(xe2x88x92{dot over (w)}y+wxwz)Rxz+({dot over (w)}x+wywz)Ryz+(xe2x88x92wx2xe2x88x92wy2)Rzzxe2x80x83xe2x80x83(32)
where
{right arrow over (a)}B=[aBxaByaBz]tm (33)
The lever arms are a measurement of the distances between the accelerometers and the center of navigation.
The direction cosine matrix is used to convert the body (instrument) frame data to the navigation frame, i.e.
{right arrow over (a)}N=CBN{right arrow over (a)}B.xe2x80x83xe2x80x83(34)
The acceleration in the body frame (see Equations (30)-(32)) is a function of [{right arrow over (w)}x]2 which is second order in xcex5 and [{dot over ({right arrow over (w)})}x] which is first order in xcex5. Therefore, to convert to the navigation frame, only a first order expansion of the direction cosine matrix (see Equation (1)) is required, i.e.
CBN≈I+xcex5 sin(wt)[{right arrow over (1)}xcfx86x].xe2x80x83xe2x80x83(35)
The full matrix multiplication in Equation (34) is not required as only DC terms are of interest. The mean acceleration in the navigation frame is given by
xe2x80x83 less than aNx greater than =xc2xdxcex52w2[cy2(Rxzxe2x88x92Rxx)+cx2(Rxyxe2x88x92Rxx)+cxcy(Ryxxe2x88x92Ryz)+cxcz(Rzxxe2x88x92Rzy)]xe2x80x83xe2x80x83(36)
 less than aNy greater than =xc2xdxcex52w2[cx2(Ryzxe2x88x92Ryy)+cz2(Ryzxe2x88x92Ryy)+cxcy(Rxyxe2x88x92Rxz)+cycz(Rzyxe2x88x92Rzx)]xe2x80x83xe2x80x83(37)
 less than aNx greater than =xc2xdxcex52w2[cx2(Rzyxe2x88x92Rzz)+cy2(Rzxxe2x88x92Rzz)+cxcz(Rxzxe2x88x92Rxy)+cycz(Ryzxe2x88x92Ryz)]xe2x80x83xe2x80x83(38)
The underscore in Equation (36) denotes the angular rate terms, while other terms are related to angular acceleration.
Since the accelerometers are not co-located, the lever arms xe2x80x9ccorruptxe2x80x9d the navigation center estimate for acceleration. Compensation is required.
The invention is a method and apparatus for improving the accuracy of an inertial navigation system. The method comprises (1) obtaining a measure of the angular velocity of a body frame of reference having a first axis, a second axis, and a third axis, (2) obtaining a measure of the acceleration of a first reference point in the direction of the first axis, a second reference point in the direction of the second axis, and a third reference point in the direction of the third axis, the first, second, and third reference points being fixed in the body frame, and (3) determining compensated acceleration values. A compensated acceleration value is the difference of the measure of acceleration of a reference point and a compensation quantity. A compensation quantity is an estimate of the portion of the acceleration of the reference point resulting from the rotation of the body frame.
The compensation quantity comprises a compensation term proportional to the vector cross product of a first vector and a second vector where the first vector is the angular velocity of the body frame and the second vector is the cross product of the angular velocity of the body frame and the vector connecting the navigation center to a reference point. An estimate of the angular velocity of the body frame is obtained by multiplying each of a plurality of measures of the angular velocity obtained at regular intervals in the past by a weight and summing the weighted measures of the angular velocity obtained during a specified time period in the past. The compensation quantity also comprises a compensation term proportional to the cross product of the angular acceleration of the body frame and the vector connecting the navigation center to a reference point. An estimate of the angular acceleration of the body frame is obtained by multiplying each of a plurality of measures of the angular velocity obtained at regular intervals in the past by a weight and summing the weighted measures of the angular velocity obtained during a specified time period in the past.
The method further comprises establishing the optimum navigation center based on a criterion of goodness. The criterion of goodness is minimal weighted acceleration error where acceleration error is a function of the direction of the angular velocity vector and weighted acceleration error is obtained by multiplying the acceleration error by a weighting function and integrating the result over all directions of the angular velocity vector.